/*
 * ChangeLog
 * Date             Author          Notes
 * 2015.09.05       Herbert         First Version
 */
#include "carriage.h"
#include <malloc.h>
#include "globe.h"
#include <string.h>
#include <signal.h>
#include <assert.h>
#include <unistd.h>
#include "ccycle.h"
#include "feedback.h"
#include <stdio.h>
#include <assert.h>
#include "cmd.h"
#include "master.h"
#include <minigui/common.h>
#include <minigui/minigui.h>
#include <minigui/gdi.h>
#include <minigui/window.h>
#include <minigui/control.h>


int ctest_check_stitch_error(CARRIAGE *crg, StitchGroup group);
int ctest_check_cam_error(CARRIAGE *crg, CamGroup group);

static void *carriage_query(void *user_data);
static int carriage_update_timer(CARRIAGE *crg);

CARRIAGE *carriage_new(MH_UDP *pudp, int handle)
{
    CARRIAGE *crg = (CARRIAGE*)malloc(sizeof(CARRIAGE));

    if (crg == NULL)
    {
        return NULL;
    }

    memset(crg, 0, sizeof(CARRIAGE));

    crg->handle = handle;

    crg->warn = 0;
    crg->reset_all = FALSE;
    crg->exit = FALSE;
    crg->cycle_flag = 0;
    crg->carriage_type = LEFT_CARRIAGE;

    crg->yc = yc_new();
    crg->sel = sel_new();
    crg->cam = cam_new();
    crg->motor = motor_new();
    crg->pudp = pudp;
    crg->flush_wnd = FALSE;

    memset(&crg->fb, 0, sizeof(FEEDBACK));
    carriage_get_state(crg);

    pthread_mutex_init(&crg->crg_mutex, NULL);
    pthread_create(&crg->th_query, NULL, carriage_query, (void*)crg);

    crg->test_object = TEST_END;
    crg->restore = RESTORE_NONE;
    return crg;
}

void carriage_close(CARRIAGE *crg)
{
    crg->exit = TRUE;
    pthread_join(crg->th_query, NULL);
    free(crg->yc);
    free(crg->sel);
    free(crg->cam);
    free(crg->motor);

    free(crg);
}

/*
 * @param crg: 机头测试对象
 */
int carriage_get_state(CARRIAGE *crg)
{
    assert(crg != NULL);
    CRGSTATE crgstate;
    int ret = 0;
    short fsink_zero = 0;
    short bsink_zero = 0;

    memset(&crgstate, 0, sizeof (CRGSTATE));

    /* 获取下位机状态 */
    ret = feedback_get_crg_state(crg->pudp, &crgstate);
    printf("ret = %d\n", ret);
    if (ret != 0)
    {
   //     return ret;
    }

    yc_set_state(crg->yc, (uchar*)crgstate.yc_state, 4);
    sel_set_state(crg->sel, (uchar*)crgstate.sel_state, 16);
    cam_set_state(crg->cam, (uchar*)crgstate.cam_state, 8);

   //8fsink_zero = sink_get_front_zero();
  //  bsink_zero = sink_get_back_zero();
    crgstate.motor_pos[0][4] += fsink_zero;
    crgstate.motor_pos[0][5] += bsink_zero;

    motor_set_pos(crg->motor, (short*)crgstate.motor_pos, 20);

    cam_set_pos(crg->cam, &crgstate.motor_pos[0][10], 4);

    return 0;
}

void carriage_set_single_cycle(CARRIAGE *crg, CCYCLE *cycle, int group, int index)
{
    assert(crg != NULL);
    assert(cycle != NULL);

    ccycle_set_state(cycle, CYCLE_READY);
    ccycle_set_group(cycle, group);
    ccycle_set_start_group(cycle, group);
    ccycle_set_end_group(cycle, group);
    ccycle_set_index(cycle, index);
    ccycle_set_start_index(cycle, index);
    ccycle_set_end_index(cycle, index);
}
void carriage_set_single_group_cycle(CARRIAGE *crg, CCYCLE *cycle, int group)
{
    assert(crg != NULL);
    assert(cycle != NULL);

    ccycle_set_state(cycle, CYCLE_READY);
    ccycle_set_group(cycle, group);
    ccycle_set_start_group(cycle, group);
    ccycle_set_end_group(cycle, group);
    ccycle_set_index(cycle, 0);
    ccycle_set_start_index(cycle, 0);
    ccycle_set_end_index(cycle, 7);
}

void carriage_set_group_cycle(CARRIAGE *crg, CCYCLE *reset_cycle, CCYCLE *cycle,
    int group, int end_index)
{
    assert(crg != NULL);
    assert(cycle != NULL);

    ccycle_set_state(reset_cycle, CYCLE_READY);
    ccycle_set_group(reset_cycle, group);
    ccycle_set_start_group(reset_cycle, group);
    ccycle_set_end_group(reset_cycle, group);
    ccycle_set_index(reset_cycle, 0);
    ccycle_set_start_index(reset_cycle, 0);
    ccycle_set_end_index(reset_cycle, end_index);

    ccycle_set_state(cycle, CYCLE_READY);
    ccycle_set_group(cycle, group);
    ccycle_set_start_group(cycle, group);
    ccycle_set_end_group(cycle, group);
    ccycle_set_index(cycle, 0);
    ccycle_set_start_index(cycle, 0);
    ccycle_set_end_index(cycle, end_index);
}


void carriage_set_all_cycle(CARRIAGE *crg, CCYCLE *reset_cycle, CCYCLE *cycle,
    int start_group, int end_group, int end_index)
{
    assert(crg != NULL);
    assert(cycle != NULL);

    ccycle_set_state(reset_cycle, CYCLE_READY);
    ccycle_set_group(reset_cycle, start_group);
    ccycle_set_start_group(reset_cycle, start_group);
    ccycle_set_end_group(reset_cycle, end_group);
    ccycle_set_index(reset_cycle, 0);
    ccycle_set_start_index(reset_cycle, 0);
    ccycle_set_end_index(reset_cycle, end_index);

    ccycle_set_state(cycle, CYCLE_READY);
    ccycle_set_group(cycle, start_group);
    ccycle_set_start_group(cycle, start_group);
    ccycle_set_end_group(cycle, end_group);
    ccycle_set_index(cycle, 0);
    ccycle_set_start_index(cycle, 0);
    ccycle_set_end_index(cycle, end_index);
}


void carriage_set_reset_all_cycle(CARRIAGE *crg, CCYCLE *reset_cycle,
    int end_group, int end_index)
{
    assert(crg != NULL);
    assert(reset_cycle != NULL);

    ccycle_set_state(reset_cycle, CYCLE_READY);
    ccycle_set_group(reset_cycle, 0);
    ccycle_set_start_group(reset_cycle, 0);
    ccycle_set_end_group(reset_cycle, end_group);
    ccycle_set_index(reset_cycle, 0);
    ccycle_set_start_index(reset_cycle, 0);
    ccycle_set_end_index(reset_cycle, end_index);
}


void carriage_init_yc_cycle(CARRIAGE *crg, int group, int index, int cycle_type)
{
    CCYCLE reset_cycle;
    CCYCLE cycle;

    ccycle_init(&reset_cycle);
    ccycle_init(&cycle);

    if (crg->reset_all)
    {
        carriage_set_reset_all_cycle(crg, &reset_cycle, MH_SYS_CAM - 1, 7);
    }
    else
    {
        switch(cycle_type)
        {
        case GROUP_CYCLE:
            carriage_set_group_cycle(crg, &reset_cycle, &cycle, crg->carriage_type * 2 + group, 7);
            break;
        case ALL_CYCLE:
            carriage_set_all_cycle(crg, &reset_cycle, &cycle, 0 + crg->carriage_type * 2,
                MH_SYS_CAM - 1 + crg->carriage_type * 2, 7);
            break;
        case SINGLE_CYCLE:
            carriage_set_single_cycle(crg, &cycle, crg->carriage_type * 2 + group, index);
            break;
        default:
            return;
        }
    }

    yc_set_cycle(crg->yc, &cycle, &reset_cycle);
    printf("cycle.group = %d, index = %d\n", cycle.group, cycle.index);
}

void carriage_init_sel_cycle(CARRIAGE *crg, int group, int index, int cycle_type)
{
    int end_group = MH_SYS_CAM * 4 - 1;

    CCYCLE reset_cycle;
    CCYCLE cycle;

    ccycle_init(&reset_cycle);
    ccycle_init(&cycle);

    sel_set_reset_all(crg->sel, FALSE);
    sel_set_all_cycle(crg->sel, FALSE);

    if (crg->reset_all)
    {
        sel_set_reset_all(crg->sel, TRUE);
#if (MH_SYS_CAM == 1)
        carriage_set_reset_all_cycle(crg, &reset_cycle, 3, 7);
#else
        carriage_set_reset_all_cycle(crg, &reset_cycle, (MH_SYS_CAM > 2) ? 15 : 7, 7);
#endif
    }
    else
    {
        switch(cycle_type)
        {
        case GROUP_CYCLE:
            carriage_set_group_cycle(crg, &reset_cycle, &cycle,
                crg->carriage_type * 8 + group, 7);
            break;
        case ALL_CYCLE:
            sel_set_all_cycle(crg->sel, TRUE);
            carriage_set_all_cycle(crg, &reset_cycle, &cycle, 0 + crg->carriage_type * 8,
                end_group + crg->carriage_type * 8, 7);
            break;
        case SINGLE_CYCLE:
            carriage_set_single_cycle(crg, &cycle, crg->carriage_type * 8 + group, index);
            break;
        default:
            return;
        }
    }

    sel_set_cycle(crg->sel, &cycle, &reset_cycle);
    printf("cycle.group = %d, index = %d\n", cycle.group, cycle.index);
}

void carriage_init_cam_cycle(CARRIAGE *crg, int group, int index, int cycle_type)
{
    int total_groups = MH_SYS_CAM * 2;

    CCYCLE reset_cycle;
    CCYCLE cycle;

    ccycle_init(&reset_cycle);
    ccycle_init(&cycle);

    cam_set_restore(crg->cam, FALSE);

    if (crg->reset_all)
    {
#if (MH_SYS_CAM == 1)
        carriage_set_reset_all_cycle(crg, &reset_cycle, 1, 5);
#else
        carriage_set_reset_all_cycle(crg, &reset_cycle, (MH_SYS_CAM > 2) ? 7 : 3, 5);
#endif
    }
    else
    {
        switch(cycle_type)
        {
        case GROUP_CYCLE:
            carriage_set_group_cycle(crg, &reset_cycle, &cycle,
                crg->carriage_type * total_groups + group, 5);
            break;
        case ALL_CYCLE:
            carriage_set_all_cycle(crg, &reset_cycle, &cycle, 0 + crg->carriage_type * total_groups,
                total_groups - 1 + crg->carriage_type * total_groups, 5);
            break;
        case SINGLE_CYCLE:
            carriage_set_single_cycle(crg, &cycle, crg->carriage_type * total_groups + group, index);
            break;
        default:
            return;
        }
    }

    cam_set_cycle(crg->cam, &cycle, &reset_cycle);
    printf("cycle.group = %d, index = %d\n", cycle.group, cycle.index);
}

void carriage_init_motor_cycle(CARRIAGE *crg, int group, int index, int cycle_type)
{
    CCYCLE reset_cycle;
    CCYCLE cycle;

    ccycle_init(&reset_cycle);
    ccycle_init(&cycle);

    motor_set_reset_all(crg->motor, FALSE);
    motor_set_restore(crg->motor, FALSE);

    if (crg->reset_all)
    {
        motor_set_reset_all(crg->motor, TRUE);
#if (MH_SYS_CAM == 1)
        carriage_set_reset_all_cycle(crg, &reset_cycle, 4, 1);
#else
        carriage_set_reset_all_cycle(crg, &reset_cycle,
            (MH_SYS_CAM > 2) ? (GNUM * 2 - 1) : (GNUM - 1), 1);
#endif
    }
    else
    {
        switch(cycle_type)
        {
        case GROUP_CYCLE:
            carriage_set_group_cycle(crg, &reset_cycle, &cycle, crg->carriage_type * GNUM + group, 1);
            break;
        case ALL_CYCLE:
            printf("init motor cycle, group = %d\n", group);
            if (group >= 4)
            {
                int end_group = 4 + crg->carriage_type * GNUM;
#if (MH_JTCS_CAM == MAC_DS_4SINK)
                end_group += 1;
#endif
                printf("end_group = %d\n", end_group);
                carriage_set_all_cycle(crg, &reset_cycle, &cycle, 4 + crg->carriage_type * GNUM,
                    end_group, 1);
            }
            else
            {
                carriage_set_all_cycle(crg, &reset_cycle, &cycle, 0 + crg->carriage_type * GNUM,
                    3 + crg->carriage_type * GNUM, 1);
            }
            break;
        case SINGLE_CYCLE:
            carriage_set_single_cycle(crg, &cycle, crg->carriage_type * GNUM + group, index);
            break;
        default:
            return;
        }
    }

    motor_set_cycle(crg->motor, &cycle, &reset_cycle);
    printf("cycle.group = %d, index = %d\n", cycle.group, cycle.index);
}

void carriage_init_cycle(CARRIAGE *crg,
    int group, int index, int cycle_type, TestObject to, BOOL start)
{
    assert(crg != NULL);

    switch (to)
    {
    case TEST_YC:
        carriage_init_yc_cycle(crg, group, index, cycle_type);
        if (start)
        {
            yc_start_timer(crg->yc, yc_exec_cycle, FALSE);
        }
        break;
    case TEST_SEL:
        carriage_init_sel_cycle(crg, group, index, cycle_type);
        if (start)
        {
            sel_start_timer(crg->sel, sel_exec_cycle, FALSE);
        }
        break;
    case TEST_CAM:
        carriage_init_cam_cycle(crg, group, index, cycle_type);
        if (start)
        {
            cam_start_timer(crg->cam, cam_exec_cycle, FALSE);
        }
        break;
    case TEST_MOTOR:
        carriage_init_motor_cycle(crg, group, index, cycle_type);
        if (start)
        {
            motor_start_timer(crg->motor, motor_exec_cycle, FALSE);
        }
        break;
    default:
        break;
    }
}

/*
 * @param crg: 机头对象
 * @param group: 组序号
 * @param index: 组内序号
 * @param cycle_type: 循环类型
 */
void carriage_request_cycle(CARRIAGE *crg,
    int group, int index, int cycle_type, TestObject to)
{
    assert(crg != NULL);

    crg->cycle_flag = (0x01 << cycle_type);
    crg->test_object = to;

    carriage_init_cycle(crg, group, index, cycle_type, to, TRUE);
}


/*
 * @param crg: 机头对象
 */
void carriage_reset_all(CARRIAGE *crg)
{
    assert(crg != NULL);
    crg->reset_all = TRUE;
    crg->cycle_flag = 0;
    crg->test_object = TEST_YC;

    carriage_init_cycle(crg, 0, 0, 0, TEST_YC, TRUE);
    carriage_init_cycle(crg, 0, 0, 0, TEST_CAM, FALSE);
    carriage_init_cycle(crg, 0, 0, 0, TEST_SEL, FALSE);
    carriage_init_cycle(crg, 0, 0, 0, TEST_MOTOR, FALSE);
}

void carriage_request_cancel_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);
    pthread_mutex_lock(&crg->crg_mutex);
    crg->cancel_cycle = TRUE;
    pthread_mutex_unlock(&crg->crg_mutex);
}

/*
 * @note: 取消循环
 */
void carriage_cancel_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);
    crg->cycle_flag = 0;
    crg->test_object = TEST_END;

    yc_stop_cycle(crg->yc);
    cam_stop_cycle(crg->cam);
    sel_stop_cycle(crg->sel);
    motor_stop_cycle(crg->motor);
}
int carriage_yc_group_exec(CARRIAGE *crg, int group, uchar state)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    carriage_set_single_group_cycle(crg, &cycle, crg->carriage_type * 2 + group);
    yc_set_cycle(crg->yc, &cycle, &reset_cycle);
    yc_set_group_state(crg->yc, state, group);
    crg->cycle_flag = 6;
    crg->test_object = TEST_YC;

    return ret;
}

int carriage_yc_exec(CARRIAGE *crg, int group, int index)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    carriage_set_single_cycle(crg, &cycle, crg->carriage_type * 2 + group, index);
    yc_set_cycle(crg->yc, &cycle, &reset_cycle);
    crg->cycle_flag = 0;
    crg->test_object = TEST_YC;

    return ret;
}

int carriage_sel_group_exec(CARRIAGE *crg, int group, uchar state)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    carriage_set_single_group_cycle(crg, &cycle, crg->carriage_type * 8 + group);

    sel_set_cycle(crg->sel, &cycle, &reset_cycle);
    sel_set_group_state(crg->sel, state, group);

    crg->cycle_flag = 6;
    crg->test_object = TEST_SEL;

    return ret;
}

int carriage_sel_exec(CARRIAGE *crg, int group, int index)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    carriage_set_single_cycle(crg, &cycle, crg->carriage_type * 8 + group, index);

    sel_set_cycle(crg->sel, &cycle, &reset_cycle);

    crg->cycle_flag = 0;
    crg->test_object = TEST_SEL;


    return ret;
}

int carriage_cam_exec(CARRIAGE *crg, int group, int index)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    carriage_set_single_cycle(crg, &cycle, crg->carriage_type * 4 + group, index);

    cam_set_cycle(crg->cam, &cycle, &reset_cycle);

    crg->cycle_flag = 0;
    crg->test_object = TEST_CAM;


    return ret;
}

int carriage_cam_group_exec(CARRIAGE *crg, int group, uchar state)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

     carriage_set_single_group_cycle(crg, &cycle, crg->carriage_type * 4 + group);

    cam_set_cycle(crg->cam, &cycle, &reset_cycle);
    cam_set_group_state(crg->cam, state, group);

    crg->cycle_flag = 6;
    crg->test_object = TEST_CAM;


    return ret;
}

int carriage_motor_group_exec(CARRIAGE *crg, int group, uchar state)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }
    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);
    carriage_set_single_group_cycle(crg, &cycle, crg->carriage_type * GNUM + group);
    motor_set_cycle(crg->motor, &cycle, &reset_cycle);
    motor_set_group_state(crg->motor, state, group);

    crg->cycle_flag = 6;
    crg->test_object = TEST_MOTOR;

    return ret;
}

int carriage_motor_exec(CARRIAGE *crg, int group, int index)
{
    assert(crg != NULL);
    int ret = 0;

    if (crg->cycle_flag != 0)
    {
        return 0;
    }

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);
    carriage_set_single_cycle(crg, &cycle, crg->carriage_type * GNUM + group, index);
    motor_set_cycle(crg->motor, &cycle, &reset_cycle);

    crg->cycle_flag = 0;
    crg->test_object = TEST_MOTOR;

    return ret;
}

void carriage_set_warn(CARRIAGE *crg, int error)
{
    assert(crg != NULL);
    switch(error)
    {
    case -1:
        crg->warn = MH_ERROR_DUALRAM;
        break;
    case -2:
        crg->warn = MH_ERROR_CAN;
        break;
    case CRGERROR_YC:
        crg->warn = MH_ERROR_YARN;
        break;
    case CRGERROR_CAM:
        crg->warn = MH_ERROR_SJDCT;
        break;
    case CRGERROR_SEL:
        crg->warn = MH_ERROR_XZQ;
        break;
    case CRGERROR_F1:
        crg->warn = MH_ERROR_DUMUF1;
        break;
    case CRGERROR_F2:
        crg->warn = MH_ERROR_DUMUF2;
        break;
    case CRGERROR_F3:
        crg->warn = MH_ERROR_DUMUF3;
        break;
    case CRGERROR_F4:
        crg->warn = MH_ERROR_DUMUF4;
        break;
    case CRGERROR_F1_R:
        crg->warn = MH_ERROR_DUMUF5;
        break;
    case CRGERROR_F2_R:
        crg->warn = MH_ERROR_DUMUF6;
        break;
    case CRGERROR_F3_R:
        crg->warn = MH_ERROR_DUMUF7;
        break;
    case CRGERROR_F4_R:
        crg->warn = MH_ERROR_DUMUF8;
        break;

    case CRGERROR_B1:
        crg->warn = MH_ERROR_DUMUB1;
        break;
    case CRGERROR_B2:
        crg->warn = MH_ERROR_DUMUB2;
        break;
    case CRGERROR_B3:
        crg->warn = MH_ERROR_DUMUB3;
        break;
    case CRGERROR_B4:
        crg->warn = MH_ERROR_DUMUB4;
        break;
    case CRGERROR_B1_R:
        crg->warn = MH_ERROR_DUMUB5;
        break;
    case CRGERROR_B2_R:
        crg->warn = MH_ERROR_DUMUB6;
        break;
    case CRGERROR_B3_R:
        crg->warn = MH_ERROR_DUMUB7;
        break;
    case CRGERROR_B4_R:
        crg->warn = MH_ERROR_DUMUB8;
        break;
    case CRGERROR_FSINK:
        crg->warn = MH_ERROR_SINKF;
        break;
    case CRGERROR_BSINK:
        crg->warn = MH_ERROR_SINKB;
        break;
    case CRGERROR_FSINK2:
        crg->warn = MH_ERROR_FSINK2;
        break;
    case CRGERROR_BSINK2:
        crg->warn = MH_ERROR_BSINK2;
        break;

    case CRGERROR_FSINK_R:
        crg->warn = MH_ERROR_RFSINK;
        break;
    case CRGERROR_BSINK_R:
        crg->warn = MH_ERROR_RBSINK;
        break;
    case CRGERROR_CAMF1:
        crg->warn = MH_ERROR_CAMF1;
        break;
    case CRGERROR_CAMB1:
        crg->warn = MH_ERROR_CAMB1;
        break;
    case CRGERROR_CAMF2:
        crg->warn = MH_ERROR_CAMF2;
        break;
    case CRGERROR_CAMB2:
        crg->warn = MH_ERROR_CAMB2;
        break;
    default:
        crg->warn = 0;
        break;
    }
}

/*
 * @ret: 1代表什么都没做
 */
int carriage_single_test(CARRIAGE *crg)
{
    int ret = 0;
    int group = 0;
    int index = 0;
    CCYCLE *cycle = NULL;
    switch (crg->test_object)
    {
    case TEST_YC:
        cycle = yc_get_cycle(crg->yc);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = yc_exec(crg->pudp, crg->yc, ACT, group, index);
        if (ret == 0)
        {
            yc_start_timer(crg->yc, yc_check_error, FALSE);
        }
        break;
    case TEST_SEL:
        cycle = sel_get_cycle(crg->sel);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = sel_exec(crg->pudp, crg->sel, ACT, group, index);
        if (ret == 0)
        {
            sel_start_timer(crg->sel, sel_check_error, FALSE);
        }
        break;
    case TEST_CAM:
        cycle = cam_get_cycle(crg->cam);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = cam_exec(crg->pudp, crg->cam, ACT, group, index);
        if (ret == 0)
        {
            cam_start_timer(crg->cam, cam_check_error, FALSE);
        }
        break;
    case TEST_MOTOR:
        cycle = motor_get_cycle(crg->motor);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = motor_exec(crg->pudp, crg->motor, ACT);
        if (ret == 0)
        {
            motor_start_timer(crg->motor, motor_check_error, FALSE);
        }
        break;
    default:
        return 1;
    }
    crg->test_object = TEST_END;
    return 0;
}

/*
 * @ret: 1代表什么都没做
 */
int carriage_group_test(CARRIAGE *crg)
{
    int ret = 0;
    int group = 0;
    int index = 0;
    CCYCLE *cycle = NULL;
    switch (crg->test_object)
    {
    case TEST_YC:
        cycle = yc_get_cycle(crg->yc);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = yc_exec_group(crg->pudp, crg->yc, group);
        if (ret == 0)
        {
            yc_start_timer(crg->yc, yc_check_error, FALSE);
        }
        break;
    case TEST_SEL:
        cycle = sel_get_cycle(crg->sel);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = sel_exec_group(crg->pudp, crg->sel, group);
        if (ret == 0)
        {
            sel_start_timer(crg->sel, sel_check_error, FALSE);
        }
        break;
    case TEST_CAM:
        cycle = cam_get_cycle(crg->cam);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = cam_exec_group(crg->pudp, crg->cam, group);
        if (ret == 0)
        {
            cam_start_timer(crg->cam, cam_check_error, FALSE);
        }
        break;
    case TEST_MOTOR:
        cycle = motor_get_cycle(crg->motor);
        group = ccycle_get_group(cycle);
        index = ccycle_get_index(cycle);
        ret = motor_exec_group(crg->pudp, crg->motor, group);
        if (ret == 0)
        {
            motor_start_timer(crg->motor, motor_check_error, FALSE);
        }
        break;
    default:
        return 1;
    }
    crg->test_object = TEST_END;
    return 0;
}
/*
 * @note: 查询感应器状态和错误状态
 */
void *carriage_query(void *user_data)
{
    CARRIAGE *crg = (CARRIAGE*)user_data;
    int ret = 0;
    FEEDBACK fb;

    memset(&fb, 0, sizeof (FEEDBACK));
    while (!crg->exit)
    {
        pthread_mutex_lock(&crg->crg_mutex);
        ret = feedback_get_sensor_state(crg->pudp, &crg->fb, crg->carriage_type);
        pthread_mutex_unlock(&crg->crg_mutex);
        if (ret == 0)
        {
            if (memcmp(&crg->fb.sensor, &fb.sensor, sizeof (fb.sensor)))
            {
                carriage_updatewindow(crg, TRUE);
            }

            memcpy(&fb, &crg->fb, sizeof (fb));

            if ((crg->warn == MH_ERROR_DUALRAM) ||
                (crg->warn == MH_ERROR_CAN))
            {
                crg->warn = 0;
            }
        }
        else
        {
            if (ret == -1)
            {
                crg->warn = MH_ERROR_DUALRAM;
            }
            else if (ret == -3)
            {
                crg->warn = MH_ERROR_CAN;
            }

            if (ret < 0)
            {
                carriage_updatewindow(crg, TRUE);
            }
        }
        usleep(5000);

        ret = carriage_update_timer(crg);

        if (ret < 0)
        {
            //printf("set warn = %d\n", ret);
            carriage_set_warn(crg, ret);
            carriage_cancel_cycle(crg);
            crg->reset_all = FALSE;
            if (crg->restore == RESTORE_REQUEST)
            {
                crg->restore = RESTORE_FAILED;
            }
        }
        else
        {
            if (ret == 0)
            {
                carriage_set_warn(crg, 0);
            }

            if (crg->reset_all)
            {
                ret = carriage_reset_all_cycle(crg);
            }
            else if (crg->restore == RESTORE_REQUEST)
            {
                ret = carriage_restore_cycle(crg);
            }
            else if (crg->cycle_flag == 0)
            {
                /* 单个测试 */
                ret = carriage_single_test(crg);
            }
            else if(crg->cycle_flag == 6)
            {
                ret = carriage_group_test(crg);
                crg->cycle_flag = 0;
            }
        }

        /* 取消循环 */
        if (crg->cancel_cycle)
        {
            carriage_cancel_cycle(crg);
            pthread_mutex_lock(&crg->crg_mutex);
            crg->cancel_cycle = FALSE;
            pthread_mutex_unlock(&crg->crg_mutex);
        }

        if (ret != 1)
        {
            carriage_updatewindow(crg, TRUE);
        }
    }

    pthread_exit(0);
}

/*
 * @param crg: 机头对象
 * @note: 获取报警码
 */
int carriage_get_warn(CARRIAGE *crg)
{
    assert(crg != NULL);

    return crg->warn;
}


void carriage_setting_pos(CARRIAGE *crg, short sink, short st)
{
    assert(crg != NULL);

    motor_setting_pos(crg->motor, st, sink);
}

/*
 * @param crg: 机头对象
 * @note: 复位全部
 */
int carriage_reset_all_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);

    if ((crg->warn == MH_ERROR_CAN) ||
        (crg->warn == MH_ERROR_DUALRAM))
    {
        crg->reset_all = FALSE;
        return FALSE;
    }

    switch(crg->test_object)
    {
    case TEST_YC:
        if (yc_reset_complete(crg->yc))
        {
            cam_start_timer(crg->cam, cam_exec_cycle, TRUE);
            crg->test_object = TEST_CAM;
            return 0;
        }
        break;
    case TEST_CAM:
        if (cam_reset_complete(crg->cam))
        {
            sel_start_timer(crg->sel, sel_exec_cycle, TRUE);
            crg->test_object = TEST_SEL;
            return 0;
        }
        break;
    case TEST_SEL:
        if (sel_reset_complete(crg->sel))
        {
            motor_start_timer(crg->motor, motor_exec_cycle, TRUE);
            crg->test_object = TEST_MOTOR;
            return 0;
        }
        break;
    case TEST_MOTOR:
        if (motor_reset_complete(crg->motor))
        {
            crg->test_object = TEST_END;
            return 0;
        }
        break;
    default:
        crg->warn = MH_ERROR_RESETDONE;
        crg->reset_all = FALSE;
        return 0;
        break;
    }

    return 1;
}

/*
 * @param crg: 机头对象
 * @note: 恢复状态
 */
int carriage_restore_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);

    switch(crg->test_object)
    {
    case TEST_YC:
        if (yc_reset_complete(crg->yc))
        {
            cam_start_timer(crg->cam, cam_restore, TRUE);
            crg->test_object = TEST_CAM;
            return 0;
        }
        break;
    case TEST_CAM:
        if (cam_restore_complete(crg->cam))
        {
            sel_start_timer(crg->sel, sel_restore, TRUE);
            crg->test_object = TEST_SEL;
            return 0;
        }
        break;
    case TEST_SEL:
        if (sel_reset_complete(crg->sel))
        {
            motor_start_timer(crg->motor, motor_exec_cycle, TRUE);
            crg->test_object = TEST_MOTOR;
            return 0;
        }
        break;
    case TEST_MOTOR:
        if (motor_restore_complete(crg->motor))
        {
            crg->test_object = TEST_END;
            return 0;
        }
        break;
    default:
        crg->restore = RESTORE_SUCCESS;
        return 0;
        break;
    }
    return 1;
}

void carriage_set_yc_restore_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    carriage_set_reset_all_cycle(crg, &reset_cycle, (MH_SYS_CAM > 2) ? 3 : 1, 7);

    yc_set_cycle(crg->yc, &cycle, &reset_cycle);
}


void carriage_set_sel_restore_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    carriage_set_reset_all_cycle(crg, &reset_cycle, (MH_SYS_CAM > 2) ? 15 : 7, 7);

    sel_set_cycle(crg->sel, &cycle, &reset_cycle);
}

void carriage_set_cam_restore_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);

    cam_set_restore(crg->cam, TRUE);

    carriage_set_reset_all_cycle(crg, &reset_cycle, (MH_SYS_CAM > 2) ? 7 : 3, 5);
#if ((MH_JTCS_CAM == MAC_DS_ZX) || (MH_JTCS_CAM == MAC_JF_SS) || (MH_JTCS_CAM == MAC_SINK_CAM))
    /* 电机款三角 */
    carriage_set_reset_all_cycle(crg, &cycle, (MH_SYS_CAM > 2) ? 7 : 3, 5);
#endif
    cam_set_cycle(crg->cam, &cycle, &reset_cycle);
}

void carriage_set_motor_restore_cycle(CARRIAGE *crg)
{
    assert(crg != NULL);

    CCYCLE cycle;
    CCYCLE reset_cycle;
    ccycle_init(&cycle);
    ccycle_init(&reset_cycle);
    motor_set_restore(crg->motor, TRUE);
    carriage_set_reset_all_cycle(crg, &reset_cycle, (MH_SYS_CAM > 2) ? 9 : 4, 1);
    carriage_set_reset_all_cycle(crg, &cycle, (MH_SYS_CAM > 2) ? 9 : 4, 1);

    motor_set_cycle(crg->motor, &cycle, &reset_cycle);
}

/*
 * @note: 恢复状态
 */
int carriage_restore(CARRIAGE *crg)
{
    if (crg->restore != RESTORE_NONE)
    {
        return FALSE;
    }

    crg->test_object = TEST_END;
    crg->reset_all = FALSE;
    crg->cycle_flag = 0;
    crg->restore = RESTORE_REQUEST;

    yc_stop_cycle(crg->yc);
    if (yc_need_restore(crg->yc))
    {
        printf("need restore yc\n");
        carriage_set_yc_restore_cycle(crg);
        if (crg->test_object == TEST_END)
        {
            crg->test_object = TEST_YC;
        }
    }

    cam_stop_cycle(crg->cam);
    if (cam_need_restore(crg->cam))
    {
        printf("need restore cam\n");
        carriage_set_cam_restore_cycle(crg);
        if (crg->test_object == TEST_END)
        {
            crg->test_object = TEST_CAM;
        }
    }

    sel_stop_cycle(crg->sel);
    if (sel_need_restore(crg->sel))
    {
        printf("need restore sel\n");
        carriage_set_sel_restore_cycle(crg);
        if (crg->test_object == TEST_END)
        {
            crg->test_object = TEST_SEL;
        }
    }

    motor_stop_cycle(crg->motor);
    if (motor_need_restore(crg->motor))
    {
        printf("need restore motor\n");
        carriage_set_motor_restore_cycle(crg);
        if (crg->test_object == TEST_END)
        {
            crg->test_object = TEST_MOTOR;
        }
    }

    if (crg->test_object == TEST_END)
    {
        return FALSE;
    }
    switch (crg->test_object)
    {
    case TEST_YC:
        yc_start_timer(crg->yc, yc_restore, TRUE);
        break;
    case TEST_CAM:
        cam_start_timer(crg->cam, cam_restore, TRUE);
        break;
    case TEST_SEL:
        sel_start_timer(crg->sel, sel_restore, TRUE);
        break;
    case TEST_MOTOR:
        motor_start_timer(crg->motor, motor_exec_cycle, TRUE);
        break;
    default:
        break;
    }

    return TRUE;
}

/*
 * @note: 更新窗口　
 */
void carriage_updatewindow(CARRIAGE *crg, int show)
{
    assert(crg != NULL);
    HWND wnd = (HWND)carriage_get_flush_data(crg);
    if(wnd)
    {
        PostMessage(wnd, MSG_IDLE, 0, 0);
    }
}

int carriage_get_flush_data(CARRIAGE *crg)
{
    assert(crg != NULL);
    return crg->flush_wnd;
}

void carriage_set_flush_data(CARRIAGE *crg, int state)
{
    assert(crg != NULL);
    crg->flush_wnd = state;
}

static int carriage_update_timer(CARRIAGE *crg)
{
    assert(crg != NULL);

    int ret = 0;

    ret = yc_exec_timer(crg->pudp, crg->yc);
    if (ret <= 0)
    {
        printf("yc exec timer: ret = %d\n", ret);
        return ret;
    }

    ret = cam_exec_timer(crg->pudp, crg->cam);
    if (ret <= 0)
    {
        printf("cam exec timer: ret = %d\n", ret);
        return ret;
    }

     ret = sel_exec_timer(crg->pudp, crg->sel);
    if (ret <= 0)
    {
        printf("sel exec timer: ret = %d\n", ret);
        return ret;
    }

    ret = motor_exec_timer(crg->pudp, crg->motor);
    if (ret <= 0)
    {
       printf("motor exec timer: ret = %d\n", ret);
        return ret;
    }


    return 1;
}

/*
 * @param crg: 机头对象
 */
void carriage_get_yc_state(CARRIAGE *crg, uchar *yc_state)
{
    assert(crg != NULL);

    uchar state[4] = { 0 };
    yc_get_state(crg->yc, state, 4);

    yc_state[0] = ~state[crg->carriage_type * 2];
    yc_state[1] = ~state[crg->carriage_type * 2 + 1];
}

/*
 * @param crg: 机头对象
 */
void carriage_get_cam_state(CARRIAGE *crg, uchar *cam_state)
{
    assert(crg != NULL);

    uchar state[8] = { 0 };
    cam_get_state(crg->cam, state, 8);

    cam_state[0] = state[crg->carriage_type * 4];
    cam_state[1] = state[crg->carriage_type * 4 + 1];
    cam_state[2] = state[crg->carriage_type * 4 + 2];
    cam_state[3] = state[crg->carriage_type * 4 + 3];
}

/*
 * @param crg: 机头对象
 */
void carriage_get_sel_state(CARRIAGE *crg, uchar *sel_state)
{
    assert(crg != NULL);

    uchar state[16] = { 0 };
    int i = 0;
    sel_get_state(crg->sel, state, 16);

    for (i = 0; i < 8; i++)
    {
        sel_state[i] = state[crg->carriage_type * 8 + i];
    }
}

/*
 * @param crg: 机头对象
 * @param motor_pos[out]: 电机位置
 * @param num: 电机个数
 */
void carriage_get_motor_pos(CARRIAGE *crg, short *motor_pos, int num)
{
    assert(crg != NULL);

    short pos[24] = { 0 };
    int i = 0;

    if (num > 12)
    {
        return;
    }

    motor_get_pos(crg->motor, pos, 24);

    for (i = 0; i < num; i++)
    {
        motor_pos[i] = pos[crg->carriage_type * 12 + i];
    }
}

/*
 * @param crg: 机头对象
 * @param state: 感应器状态
 * 0-9:度目前１,前２,后１,后２，度目前３，前４，后３，后４，前辛克，后辛克
 * 10-13: 前三角零位１，前三角零位２，后三角零位１，后三角零位２
 * @param num: 感应器个数
 */
void carriage_get_sensor_state(CARRIAGE *crg, char *state, int num)
{
    assert(crg != NULL);

    if (num < 20)
    {
        return;
    }

#if(MH_SYS_CAM == 1)
    state[0] = crg->fb.sensor.q1;
    state[1] = crg->fb.sensor.q2;
    state[2] = crg->fb.sensor.h1;
    state[3] = crg->fb.sensor.h2;
    /* state[4] = crg->fb.sensor.q3;
    state[5] = crg->fb.sensor.q4;
    state[6] = crg->fb.sensor.h3;
    state[7] = crg->fb.sensor.h4;
    state[8] = crg->fb.sensor.qxk;
    state[9] = crg->fb.sensor.hxk;
    state[10] = crg->fb.sensor.fcam1;
    state[11] = crg->fb.sensor.bcam1;
    state[12] = crg->fb.sensor.fcam2;
    state[13] = crg->fb.sensor.bcam2;*/
#else
    state[0] = crg->fb.sensor.q1;
    state[1] = crg->fb.sensor.q2;
    state[2] = crg->fb.sensor.h1;
    state[3] = crg->fb.sensor.h2;
    state[4] = crg->fb.sensor.q3;
    state[5] = crg->fb.sensor.q4;
    state[6] = crg->fb.sensor.h3;
    state[7] = crg->fb.sensor.h4;
    state[8] = crg->fb.sensor.qxk;
    state[9] = crg->fb.sensor.hxk;
    state[10] = crg->fb.sensor.fcam1;
    state[11] = crg->fb.sensor.bcam1;
    state[12] = crg->fb.sensor.fcam2;
    state[13] = crg->fb.sensor.bcam2;
#endif

    state[14] = crg->fb.sensor.fcam1_status;
    state[15] = crg->fb.sensor.bcam1_status;
    state[16] = crg->fb.sensor.fcam2_status;
    state[17] = crg->fb.sensor.bcam2_status;
    state[18] = crg->fb.sensor.qxk2;
    state[19] = crg->fb.sensor.hxk2;
}

void carriage_set_carriage_type(CARRIAGE *crg, int type)
{
    assert(crg != NULL);
    crg->carriage_type = type;
}

void carriage_set_sink_enable(CARRIAGE *crg, BOOL sink_enable)
{
    assert(crg != NULL);
    if (crg->motor != NULL)
    {
        motor_set_sink_enable(crg->motor, sink_enable);
    }
}

int carriage_get_cycle_flag(CARRIAGE *crg)
{
    assert(crg != NULL);
    return crg->cycle_flag;
}

int carriage_get_reset_all(CARRIAGE *crg)
{
    assert(crg != NULL);
    return crg->reset_all;
}

int carriage_get_restore(CARRIAGE *crg)
{
    assert(crg != NULL);
    return crg->restore;
}

void  carriage_set_restore(CARRIAGE *crg, int data)
{
    assert(crg != NULL);
    crg->restore = data;
}

int carriage_get_test_object(CARRIAGE *crg)
{
    assert(crg != NULL);
    return crg->test_object;
}



